CON
  _clkmode = xtal1 + pll16x                                               
  _xinfreq = 5_000_000
  FORWARD  = 1
  REVERSE  = 2
  LEFT     = 3
  RIGHT    = 4
  STOP     = 5
  
OBJ
  functions : "Functions"
  PS2 : "PS2_Controller"
    
PUB Main
  functions.initialize
  ps2.start(12,5000)
  repeat
    PS2_Interpret
PUB PS2_Interpret
    waitcnt(clkfreq/4 + cnt)
    if (PS2.get_RightY <> 0) &(PS2.get_RightY <> 255) & (PS2.get_RightX <> 0) & (PS2.get_RightX <> 255)
      functions.drive(STOP) 
    else
      if (PS2.get_RightY == 255)
        functions.drive(REVERSE)
      if (PS2.get_RightY == 0)
        functions.drive(FORWARD)
      if (PS2.get_RightX == 255)
        functions.drive(RIGHT)
      if (PS2.get_RightX == 0)
        functions.drive(LEFT)
    
       